#include "odeworldwrap.h"
#include "transformable.h"
#include "physical.h"
#include <ode/objects.h>

namespace CoreBase
{
	ODEWorldWrap::ODEWorldWrap() : _worldID(0)
		, _gravity(0.f, 0.f, 0.f)
	{
		_worldID = dWorldCreate();
		setGravity(0.f, 0.f, -9.81f);
	}

	ODEWorldWrap::~ODEWorldWrap()
	{
		dWorldDestroy(_worldID);
	}

	dWorldID ODEWorldWrap::getWorldID() const
	{
		return _worldID;
	}

	void ODEWorldWrap::step(const double &stepSize)
	{
		dWorldQuickStep(_worldID, stepSize);
	}

	void ODEWorldWrap::setGravity(float x, float y, float z)
	{
		setGravity( osg::Vec3(x,y,z) );
	}

	void ODEWorldWrap::getGravity(osg::Vec3& vec) const
	{
		vec = _gravity;
	}

	osg::Vec3 ODEWorldWrap::getGravity() const
	{
		return _gravity;
	}

	void ODEWorldWrap::getGravity(float &x, float &y, float &z) const
	{
		x = _gravity[0]; y = _gravity[1]; z = _gravity[2];
	}

	void ODEWorldWrap::setGravity( const osg::Vec3& gravity )
	{
		_gravity.set(gravity);

		dWorldSetGravity(_worldID, _gravity[0], _gravity[1], _gravity[2]);
	}

	void ODEWorldWrap::registerCollidable(Transformable* collidable)
	{
		// This should probably be some sort of virtual function.
		// Or perhaps RegisterPhysical can stick around and only do
		// this.
		if ( Physical* physical = dynamic_cast<Physical*>(collidable) )
		{
			if (physical != NULL)
			{
				physical->setBodyID( dBodyCreate( _worldID ) );
			}
		}
	}

	void ODEWorldWrap::unRegisterCollidable(Transformable* collidable)
	{
		if (Physical* physical = dynamic_cast<Physical*>(collidable))
		{
			if (physical != NULL)
			{
				physical->setBodyID(0);
			}
		}
	}

	void ODEWorldWrap::setDamping(float linearScale, float angularScale)
	{
		dWorldSetDamping(_worldID, linearScale, angularScale);
	}

	float ODEWorldWrap::getLinearDamping() const
	{
		return dWorldGetLinearDamping(_worldID);
	}

	float ODEWorldWrap::getAngularDamping() const
	{
		return dWorldGetAngularDamping(_worldID);
	}

	void ODEWorldWrap::setLinearDampingThreshold(float linearThreshold)
	{
		dWorldSetLinearDampingThreshold(_worldID, linearThreshold);
	}

	float ODEWorldWrap::getLinearDampingThreshold() const
	{
		return dWorldGetLinearDampingThreshold(_worldID);
	}

	void ODEWorldWrap::setAngularDampingThreshold(float angularThreshold)
	{
		dWorldSetAngularDampingThreshold(_worldID, angularThreshold);
	}

	float ODEWorldWrap::getAngularDampingThreshold() const
	{
		return dWorldGetAngularDampingThreshold(_worldID);
	}
}